Load sympy
In [1]:
from sympy import *
from __future__ import division, print_function, with_statement
init_printing()
Initialize variables
In [2]:
p0 = Matrix(symbols('p0[0:3]', real=True))
p1 = Matrix(symbols('p1[0:3]', real=True))
p2 = Matrix(symbols('p2[0:3]', real=True))
p3 = Matrix(symbols('p3[0:3]', real=True))
q0 = Matrix(symbols('q0[0:3]', real=True))
q1 = Matrix(symbols('q1[0:3]', real=True))
q2 = Matrix(symbols('q2[0:3]', real=True))
q3 = Matrix(symbols('q3[0:3]', real=True))
u = Matrix(((p0 - p3).T, (p1 - p3).T, (p2 - p3).T)).T
v = Matrix(((q0 - q3).T, (q1 - q3).T, (q2 - q3).T)).T
uInv = Matrix((symbols('uInv[0][(0:3)]', real=True),
symbols('uInv[1][(0:3)]', real=True),
symbols('uInv[2][(0:3)]', real=True)))
m = Matrix((symbols('m[0][(0:3)]', real=True),
symbols('m[1][(0:3)]', real=True),
symbols('m[2][(0:3)]', real=True)))
t = Matrix(symbols('t[0:3]', real=True))
Ai = symbols('Ai', real=True)
alpha = symbols('alpha', real=True)
Error expression for 1 triangle
In [3]:
Ei = Ai * ((v * uInv - m).norm() ** 2 + alpha * ((q3 - v * uInv * p3) - t).norm() ** 2)
In [4]:
Ei
Out[4]:
Compute equations from partial derivatives (this takes a while...)
In [5]:
eqs = []
q = [q0, q1, q2, q3]
for qi in q:
#for qi in [q0]:
for x in (0, 1, 2):
#for x in (0,):
qx = []
coeffs = {}
for qj in q:
qx.append(qj[x])
coeffs[qj[x]] = Integer(0)
Eid = Ei.diff(qi[x]).expand().collect(qx)
eqA = Matrix(map(lambda qjx: Eid.coeff(qjx).factor(), qx)).T
eqX = Matrix(qx)
eqB = Matrix([-Eid.as_independent(*qx)[0].factor()])
eqs.append((eqA, eqX, eqB))
In [6]:
len(eqs)
Out[6]:
Custom C code printer to avoid using pow with integers (took from here)
In [8]:
from sympy.utilities.codegen import CCodePrinter
class MyCCodePrinter(CCodePrinter):
def _print_Pow(self, expr):
if expr.exp.is_integer and expr.exp.is_number:
return '(' + '*'.join([self._print(expr.base)]*expr.exp) + ')'
else:
return super(MyCCodePrinter, self)._print_Pow(expr)
Format as C code
In [11]:
import re
cPrinter = MyCCodePrinter()
varRe = re.compile(r'(\w)\s*(\d+)\s*\[\s*(\d+)\s*\]')
idx2Re = re.compile(r'\[\s*(\d+)\s*\]\s*\[\s*(\d+)\s*\]')
idx1Re = re.compile(r'\[\s*(\d+)\s*\]')
with open('include/surfmorph/surfmorph_codegen.h', 'w') as f:
f.write('\n')
f.write('////////////////////////////////////////////////////////////////////////////////\n')
f.write('// This code has been automatically generated with SymPy ///////////////////////\n')
f.write('////////////////////////////////////////////////////////////////////////////////\n')
f.write('\n')
f.write('#ifndef SURFMORPH_CODEGEN_H\n')
f.write('#define SURFMORPH_CODEGEN_H\n')
f.write('\n')
f.write('namespace surfmorph\n')
f.write('{\n')
f.write('\n')
f.write('template<typename ScalarT,\n')
f.write(' typename IdxT,\n')
f.write(' typename PointT,\n')
f.write(' typename VectorT,\n')
f.write(' typename MatrixT,\n')
f.write(' typename CoeffsT,\n')
f.write(' typename IndepsT>\n')
f.write('void updateInterpolationSystem\n')
f.write('(\n')
# Turns out only q3 is necessary as parameter
for qi in q[:-1]:
f.write(' const PointT &,\n')
f.write(' const PointT &{0},\n'.format(varRe.sub(r'p\2', str(q[-1][0]))))
for qi in q:
f.write(' const IdxT {0},\n'.format(varRe.sub(r'p\2Idx', str(qi[0]))))
f.write(' const MatrixT &{0},\n'.format(str(uInv[0]).split('[')[0]))
f.write(' const MatrixT &{0},\n'.format(str(m[0]).split('[')[0]))
f.write(' const VectorT &{0},\n'.format(str(t[0]).split('[')[0]))
f.write(' const ScalarT {0},\n'.format(str(Ai)))
f.write(' const ScalarT {0},\n'.format(str(alpha)))
f.write(' CoeffsT &coeffs,\n')
f.write(' IndepsT &indeps\n')
f.write(')\n')
f.write('{\n')
f.write('\n')
f.write(' // Make sure coeff and indeps data is allocated\n')
f.write('#ifdef SURFMORPH_PARALLELIZE_INTERPOLATION\n')
f.write('#pragma omp single\n')
f.write('#endif\n')
f.write(' {\n')
qVars = sum(map(list, q), [])
for i, (qi, eq) in enumerate(zip(qVars, eqs)):
eqName = varRe.sub(r'3 * p\2Idx + \3', str(qi))
for qj in eq[1]:
termName = varRe.sub(r'3 * p\2Idx + \3', str(qj))
f.write(' coeffs[{{{0}, {1}}}] = coeffs[{{{0}, {1}}}];\n'.format(eqName, termName))
f.write(' indeps[{0}] = indeps[{0}];\n'.format(eqName))
f.write(' }\n')
f.write('\n')
f.write('#ifdef SURFMORPH_PARALLELIZE_INTERPOLATION\n')
f.write('#pragma omp sections\n')
f.write('#endif\n')
f.write(' {\n\n')
for i, (qi, eq) in enumerate(zip(qVars, eqs)):
eqName = varRe.sub(r'3 * p\2Idx + \3', str(qi))
f.write('#ifdef SURFMORPH_PARALLELIZE_INTERPOLATION\n')
f.write('#pragma omp section\n')
f.write('#endif\n')
f.write(' {\n')
f.write(' // Equation {0}\n'.format(i + 1))
for qj, a in zip(eq[1], eq[0]):
termName = varRe.sub(r'3 * p\2Idx + \3', str(qj))
eqCode = cPrinter.doprint(a)
eqCode = idx2Re.sub(r'(\1, \2)', eqCode)
eqCode = idx1Re.sub(r'(\1)', eqCode)
f.write(' coeffs[{{{0}, {1}}}] += {2};'.format(eqName, termName, eqCode))
f.write('\n')
indepCode = cPrinter.doprint(eq[2][0])
indepCode = idx2Re.sub(r'(\1, \2)', indepCode)
indepCode = idx1Re.sub(r'(\1)', indepCode)
f.write(' indeps[{0}] += {1};'.format(eqName, indepCode))
f.write('\n')
f.write(' }\n\n')
f.write(' }\n')
f.write('}\n\n')
f.write('}\n\n')
f.write('#endif\n\n')
f.write('////////////////////////////////////////////////////////////////////////////////\n')
Save equations in Latex file
In [7]:
eqLatex = map(lambda eq: '\\begin{{equation*}}\n{0}^T\n{1}\n=\n{2}\n\\end{{equation*}}\n\\clearpage\n'\
.format(latex(eq[0].T), latex(eq[1]), latex(eq[2])), eqs)
with open('eqs.tex', 'w') as f:
f.writelines(eqLatex)